#include "../Test/Test.h"
#include "../../src/Math/Fixed.h"

#include <iostream>

namespace uav
{
namespace test
{

using namespace math;

class FixedTest : public Suite
{
public:
	FixedTest()
	{
		TEST_ADD(FixedTest::values);
		TEST_ADD(FixedTest::conversion);
		TEST_ADD(FixedTest::add_sub);
		TEST_ADD(FixedTest::div_mul);
		TEST_ADD(FixedTest::sqrt_exp_log);
		TEST_ADD(FixedTest::trig);
		TEST_ADD(FixedTest::inv_trig);
		TEST_ADD(FixedTest::compare);
		TEST_ADD(FixedTest::misc);
	}

protected:
	virtual void setup()
	{
		zero = Fixed();
		one = Fixed(1);
		a = Fixed(48231.253);
		b = Fixed(-0.39756);
		c = Fixed("0.02598");
		d = Fixed("-108792.5");
		e = Fixed(10.79942);
	}

	virtual void tear_down()
	{
	}

private:
	Fixed zero, one, a, b, c, d, e;

	void values()
	{
		TEST_ASSERT_EQUAL(0, zero.bits());
		TEST_ASSERT_EQUAL(1 << 28, one.bits());
		TEST_ASSERT_DELTA(12946978392506LL, a.bits(), 1);
		TEST_ASSERT_DELTA(-106719200, b.bits(), 1);
		TEST_ASSERT_DELTA(6973953, c.bits(), 1);
		TEST_ASSERT_DELTA(-29203764346880LL, d.bits(), 1);
	}

	void conversion()
	{
		TEST_ASSERT_DELTA(48231.253, a.toDouble(), 0.0000001);
		TEST_ASSERT_DELTA( -0.39756, b.toDouble(), 0.0000001);
		TEST_ASSERT_DELTA(  0.02598, c.toDouble(), 0.0000001);
		TEST_ASSERT_DELTA(-108792.5, d.toDouble(), 0.0000001);
		TEST_ASSERT_EQUAL("-108792.5", d.toString());
	}

	void add_sub()
	{
		TEST_ASSERT_EQUAL(0, (one - one).bits());
		TEST_ASSERT_EQUAL(one, one - zero);
		TEST_ASSERT_EQUAL(one, one + zero);
		TEST_ASSERT_EQUAL(Fixed(2), one + one);
		TEST_ASSERT_DELTA(-60561.247, (a + d).toDouble(), 0.0000001);
		TEST_ASSERT_DELTA(  -0.42354, (b - c).toDouble(), 0.0000001);
	}

	void div_mul()
	{
		TEST_ASSERT_EQUAL(zero, one * zero);
		TEST_ASSERT_EQUAL(one, one * one);
		TEST_ASSERT_EQUAL(zero, a * zero);
		TEST_ASSERT_DELTA(      48231.253, (a * one).toDouble(), 0.0000001);
		TEST_ASSERT_DELTA(-19174.81694268, (a * b).toDouble(),   0.0005);
		TEST_ASSERT_DELTA(     43251.5463, (b * d).toDouble(),   0.0005);

		TEST_ASSERT_EQUAL(zero, zero / one);
		TEST_ASSERT_DELTA(   -2.255643244, (d / a).toDouble(),   0.0000001);
		TEST_ASSERT_DELTA(   -0.065348627, (c / b).toDouble(),   0.0000001);
	}

	void sqrt_exp_log()
	{
		TEST_ASSERT_EQUAL(zero, zero.sqrt());
		TEST_ASSERT_EQUAL(one, one.sqrt());
		TEST_ASSERT_DELTA(219.616149224, a.sqrt().toDouble(), 0.0000001);
		TEST_ASSERT_DELTA(  0.161183126, c.sqrt().toDouble(), 0.0000001);

		TEST_ASSERT_EQUAL(one, zero.exp());
		TEST_ASSERT_DELTA(    2.718281828, one.exp().toDouble(), 0.0000001);
		TEST_ASSERT_DELTA(    0.671957624, b.exp().toDouble(),   0.0000001);
		TEST_ASSERT_DELTA(48992.377315428, e.exp().toDouble(),   0.0005);

		TEST_ASSERT_EQUAL(zero, one.log());
		TEST_ASSERT_DELTA( 1.000000000, Fixed(2.718281828).log().toDouble(), 0.0000001);
		TEST_ASSERT_DELTA(10.783762492, a.log().toDouble(),                  0.0000001);
		TEST_ASSERT_DELTA(-3.650428268, c.log().toDouble(),                  0.0000001);

	}

	void trig()
	{
		TEST_ASSERT_DELTA(0.0, zero.sin().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(1.0, zero.cos().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(0.0, zero.tan().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(1.0, Fixed::HalfPi.sin().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(0.0, Fixed::HalfPi.cos().toDouble(), 0.000001);
		TEST_ASSERT_DELTA( 0.0, Fixed::Pi.sin().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(-1.0, Fixed::Pi.cos().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(0.0, Fixed::TwoPi.sin().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(1.0, Fixed::TwoPi.cos().toDouble(), 0.000001);

		Fixed sinC, cosC;
		c.sinCos(sinC, cosC);
		TEST_ASSERT_DELTA(0.025977078, sinC.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(0.999662539, cosC.toDouble(), 0.000001);

		Fixed sinB, cosB;
		b.sinCos(sinB, cosB);
		TEST_ASSERT_DELTA(-0.387169796, sinB.toDouble(), 0.000001);
		TEST_ASSERT_DELTA( 0.922008432, cosB.toDouble(), 0.000001);
	}

	void inv_trig()
	{
		TEST_ASSERT_DELTA(0.0, zero.atan().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(Fixed::QuarterPi.toDouble(), one.atan().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(1.569796327, Fixed(1000).atan().toDouble(), 0.000001);
		TEST_ASSERT_DELTA(-0.463647609, (-Fixed::Half).atan().toDouble(), 0.000001);

		TEST_ASSERT_DELTA(1.478462062, Fixed::atan2(e, Fixed::One).toDouble(), 0.000001);
		TEST_ASSERT_DELTA(-1.478462062, Fixed::atan2(-e, Fixed::One).toDouble(), 0.000001);
		TEST_ASSERT_DELTA(1.663130592, Fixed::atan2(e, -Fixed::One).toDouble(), 0.000001);
		TEST_ASSERT_DELTA(-1.663130592, Fixed::atan2(-e, -Fixed::One).toDouble(), 0.000001);

		Fixed r, theta;
		Fixed::toPolar(Fixed::One, Fixed::One, r, theta);
		TEST_ASSERT_DELTA(1.414213562, r.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(Fixed::QuarterPi.toDouble(), theta.toDouble(), 0.000001);

		Fixed::toPolar(Fixed(-3), Fixed(4), r, theta);
		TEST_ASSERT_DELTA(5.0, r.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(2.214297436, theta.toDouble(), 0.000001);
	}

	void compare()
	{
		TEST_ASSERT(Fixed::One == Fixed::One);
		TEST_ASSERT(a != b);
		TEST_ASSERT(a > b);
		TEST_ASSERT(Fixed::Zero <= Fixed::Zero);
		TEST_ASSERT(b < c);
		TEST_ASSERT(Fixed::Pi >= Fixed::Pi);
		TEST_ASSERT(Fixed::TwoPi >= Fixed::HalfPi);
	}

	void misc()
	{
		TEST_ASSERT_EQUAL(Fixed(10), e.floor());
		TEST_ASSERT_EQUAL(Fixed(11), e.ceil());
		TEST_ASSERT_EQUAL(Fixed::One, Fixed::One.floor());
		TEST_ASSERT_EQUAL(Fixed::One, Fixed::One.ceil());
		TEST_ASSERT_EQUAL(Fixed::Zero, b.ceil());
		TEST_ASSERT_EQUAL(-Fixed::One, b.floor());
		TEST_ASSERT_EQUAL(-Fixed::One, (-Fixed::One).floor());
		TEST_ASSERT_EQUAL(-Fixed::One, (-Fixed::One).ceil());

		TEST_ASSERT_EQUAL(a, a.abs());
		TEST_ASSERT_EQUAL(-b, b.abs());

		TEST_ASSERT_EQUAL(Fixed::One, Fixed::max(Fixed::One, Fixed::Zero));
		TEST_ASSERT_EQUAL(Fixed::Zero, Fixed::min(Fixed::One, Fixed::Zero));

		TEST_ASSERT_EQUAL(Fixed::Two, Fixed::One << 1);
		TEST_ASSERT_EQUAL(Fixed::Quarter, Fixed::One >> 2);
		TEST_ASSERT_EQUAL(Fixed(-4), (-Fixed::One) << 2);
		TEST_ASSERT_EQUAL(-Fixed::Half, (-Fixed::One) >> 1);
	}
};

}}

using namespace uav::test;

int main()
{
	TextOutput output(TextOutput::Verbose);
	FixedTest suite;
	return suite.run(output);
}
